#include "EndoscopeLogic.h"
#include "IfImg.h"
#include <fstream>
#include <vtkImageData.h>
#include <vtkUnsignedCharArray.h>
#include <vtkPointdata.h>
#include <vtkKWTkUtilities.h>
#include <process.h>
#include <vtkKWMultiColumnlist.h>
#include "aurorer/NDIControl.h"
#include "testGUI.h"
#include "endoscopewidget.h"
#include "vtkMRMLLinearTransformNode.h"
#include <vtkKWMultiColumnListWithScrollBars.h>
#include <sstream>
#include "aurorer/conversions.h"
#include "../config.h"
#include "vtkMRMLModelNode.h"
#include "vtkMRMLMyModelDisplayNode.h"
#include <vtkSTLReader.h>
vtkStandardNewMacro(EndoscopeLogic);
vtkCxxRevisionMacro(EndoscopeLogic, "$Revision: 1.0 $");
EndoscopeLogic::EndoscopeLogic(void)
{

	ImageCardHandle = INVALID_HANDLE_VALUE;
	Buffer = new char[640 * 480 * 3];
	Application = NULL;
	NDIControl = NULL;
	ModuleGUI = NULL;
	Matrix1 = NULL;
	IsCalibrating = 0;
	CvImageData = NULL;
	CvImage = NULL;
	Pivot = PivotCalibrationType::New();
	Pivot->RequestReset();
}

EndoscopeLogic::~EndoscopeLogic(void)
{
	if (Buffer != NULL)
		delete[] Buffer;

	if (NDIControl != NULL)
		delete NDIControl;
	if (Matrix1 != NULL)
		Matrix1->Delete();
	if (CvImage != NULL)
	{
		delete CvImage;
		delete[] CvImageData;
	}

	
	/*AuroraTransformsType::iterator it = AuroraTransformNodeVec.begin();
	for (it; it != AuroraTransformNodeVec.end(); ++it)
	{
		it->second->Delete();
	}*/

	SetApplication(NULL);
	CloseDevice();
}
extern ofstream fout;
 void EndoscopeLogic::ProcessMRMLEvents ( vtkObject * caller, 
								unsigned long event, void * callData )
 {

 }

 void EndoscopeLogic::ProcessLogicEvents( vtkObject * caller, 
								unsigned long event, void * callData)
 {

 }
 std::ofstream fout("f://out.txt");
 VOID CALLBACK lpCallBackProc(IMGEVENTTABLE Event, DWORD dwFrameCnt, DWORD dwUser)
 {
	
 }


 void EndoscopeLogic::StartCapture(void)
 {

	/*IMGEVENTTABLE Mask;
	Mask.dwDI = 0x00;
	Mask.dwFrame = 0x01;
	Mask.dwTrigger = 0;
	if (IFIMG_ERROR_SUCCESS != ImgSetEventMask(ImageCardHandle, &Mask))
	{
		MessageBox(NULL, "SetEventMask Error!", "Error", MB_OK);
	}
	IMGEVENTREQ Event;
	Event.hWnd = NULL;
	Event.uMessage = NULL;
	Event.hEvent = NULL;
	Event.pCallBackProc = (PIMGCALLBACK)lpCallBackProc;
	Event.dwUser = (DWORD)(void *)(this);
	if (IFIMG_ERROR_SUCCESS != ImgSetEvent(ImageCardHandle, &Event))
	{
		MessageBox(NULL, "SetEvent Error!", "Error", MB_OK);
	}
	if (IFIMG_ERROR_SUCCESS != ImgStartCapture(ImageCardHandle, 0, IFIMG_DMACAPTURE_START))
	{
		MessageBox(NULL, "Start Capture Error!", "Error", MB_OK);
	}*/
 }

 void EndoscopeLogic::StopCapture(void)
 {
	 //IMGCAPSTATUSEX status;
	 //ImgGetCaptureStatusEx (ImageCardHandle, &status);
	 if (IFIMG_ERROR_SUCCESS != ImgStopCapture(ImageCardHandle, IFIMG_FRAME_STOP))
	 {
		 MessageBox(NULL, "Stop Capture Error!", "Error", MB_OK);
	 }
 }

 void EndoscopeLogic::OpenDevice(const char * deviceName)
 {
	 //////////////////////////////////////////////////////////////////////////
	 // Aurorer Test
     //CNDIControl *cc = new CNDIControl;
	 //cc->StartInitialize();
	 ImageCardHandle = ImgOpen(deviceName);
	 if (ImageCardHandle == INVALID_HANDLE_VALUE)
	 {
		 MessageBox(NULL, "Can't Open Image Card Device!", "Error", MB_OK);
		 return;
	 }
	 else
	 {
		 MessageBox(NULL, " Open Image Card Device Successfully!", "OK", MB_OK);
	 }

	 IMGBUFFERINFO bufinfo;
	 bufinfo.pBufferAddress = Buffer;
	 bufinfo.dwBufferSize = 640 * 480 * 3;
	 if (IFIMG_ERROR_SUCCESS != ImgSetBuffer(ImageCardHandle, &bufinfo, IFIMG_COLOR_RGB24))
	 {
		 MessageBox(NULL, "Buffer Set Error!", "Error", MB_OK);
	 }
	 cvNamedWindow("Endoscope Image");
	 if (CvImage == NULL)
	 {
		 CvImage = new IplImage;
		 CvImageData = new char[640 * 480 * 3];
		 cvInitImageHeader( CvImage, cvSize(640, 480),
			 IPL_DEPTH_8U, 3, IPL_ORIGIN_BL, 4 );
		 CvImage->imageData = CvImageData;
		 //cvSetData( &src, hdr->lpData, src.widthStep );
	 }
 }

 void EndoscopeLogic::CloseDevice(void)
 {
	 if (ImageCardHandle != INVALID_HANDLE_VALUE)
	 {
		 ImgClose(ImageCardHandle);
		 ImageCardHandle = INVALID_HANDLE_VALUE;
	 }
	 cvDestroyWindow("Endoscope Image");
 }


 int EndoscopeLogic::InitializeAurorer(void)
 {
	 // this routine can be only executed once. later maybe modification is needed. wang

	 if (NDIControl == NULL)
		 NDIControl = new CNDIControl;
	int ret = NDIControl->StartInitialize();

	CCommandHandling * pCommandHandling = NDIControl->m_pCmdHandling;
	int toolPort = 0;
	for ( int i = 0; i < NO_HANDLES; i ++ )
	{
		if ( pCommandHandling->m_dtHandleInformation[i].HandleInfo.bInitialized > 0 &&
			pCommandHandling->m_dtHandleInformation[i].szToolType[1] != '8' )
		{
				++toolPort;
				using namespace std;
				stringstream sstr;
				sstr << "AuroraTransform " << pCommandHandling->m_dtHandleInformation[i].szPhysicalPort;
				/*vtkCollection * transformCollection = GetMRMLScene()->GetNodesByName(sstr.str().c_str());
				if (transformCollection->GetNumberOfItems() != 0)
				{
					AuroraTransformNodeVec[i] = vtkMRMLLinearTransformNode::SafeDownCast(transformCollection->GetItemAsObject(0));
					continue;
				}*/
				vtkMRMLLinearTransformNode * tnode = vtkMRMLLinearTransformNode::New();
				tnode->SetScene(GetMRMLScene());
				tnode->SetName(sstr.str().c_str());
				GetMRMLScene()->AddNode(tnode);
				AuroraTransformNodeVec[i] = tnode;
				vtkMRMLModelNode * mnode = vtkMRMLModelNode::New();
				vtkMRMLMyModelDisplayNode * dnode = vtkMRMLMyModelDisplayNode::New();
				sstr << " Virtual Tool " ;
				
				dnode->SetScene(GetMRMLScene());
				GetMRMLScene()->AddNodeNoNotify(dnode);
				vtkSTLReader * reader = vtkSTLReader::New();
				switch(toolPort)
				{
				case 1:
					dnode->SetColor(1, 0, 0);
					reader->SetFileName(USER_DATA "tool.stl"); 
					break;
				case 2:
					dnode->SetColor(0, 1, 0);
					reader->SetFileName(USER_DATA "tool.stl");
					break;
				case 3:
					dnode->SetColor(0, 0, 1);
					reader->SetFileName(USER_DATA "tool.stl");
					break;
				case 4:
					dnode->SetColor(1, 1, 0);
					reader->SetFileName(USER_DATA "tool.stl");
					break;
				}
				mnode->SetName(sstr.str().c_str());
				mnode->SetScene(GetMRMLScene());
				mnode->AddAndObserveDisplayNodeID(dnode->GetID());
				mnode->SetAndObserveTransformNodeID(tnode->GetID());
				reader->Update();
				mnode->SetAndObservePolyData(reader->GetOutput());
				reader->Delete();
				GetMRMLScene()->AddNode(mnode);
				
				mnode->Delete();
				dnode->Delete();
				tnode->Delete();
		}
	}
	return ret;
 }

 void EndoscopeLogic::StartTracking()
 {
	 NDIControl->StartTracking();
	 Grasp();
 }

 void EndoscopeLogic::StopTracking(void)
 {
	 //vtkKWTkUtilities::CancelTimerHandler(GetApplication(), graspTimerID.c_str());
	 NDIControl->StopTracking();
 }

 void EndoscopeLogic::Grasp(void)
 {
	 char
		 pszTemp[256];		

	 CCommandHandling * pCommandHandling = NDIControl->m_pCmdHandling;
	 if (!NDIControl->m_IsTracking)
		 return ;



	 /* 
	 * if tracking mode is 0, we are asking for TX data, else we are
	 * asking for BX data.
	 */
	 if ( NDIControl->m_nTrackingMode == 0 )
	 {
		 if ( !pCommandHandling->nGetTXTransforms( true ) )
			 return ;
	 } /* if */
	 else if (NDIControl-> m_nTrackingMode == 1 )
	 {
		 if ( !pCommandHandling->nGetBXTransforms( true ) )
			 return ;
	 } /* else if */

	 /* check for system flags */
	 m_bInterference = pCommandHandling->m_dtSystemInformation.bTooMuchInterference;
	 m_bPortOccupied = pCommandHandling->m_dtSystemInformation.bPortOccupied;
	 m_bPortUnoccupied = pCommandHandling->m_dtSystemInformation.bPortUnoccupied;

	int lineNum = -1;
	 for ( int i = 0; i < NO_HANDLES; i ++ )
	 {
		 if ( pCommandHandling->m_dtHandleInformation[i].HandleInfo.bInitialized > 0 &&
			 pCommandHandling->m_dtHandleInformation[i].szToolType[1] != '8' )
		 {
			 /* only update the frame if the handle isn't disabled*/
			 if ( pCommandHandling->m_dtHandleInformation[i].Xfrms.ulFlags == TRANSFORM_VALID ||
				 pCommandHandling->m_dtHandleInformation[i].Xfrms.ulFlags == TRANSFORM_MISSING )
			 {
		
					// pCommandHandling->m_dtHandleInformation[i].Xfrms.ulFrameNumber 
				 ++lineNum;
				 sprintf( pszTemp, "%0x", i);
				 ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 0, pszTemp);
				 ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 1, pCommandHandling->m_dtHandleInformation[i].szPhysicalPort);
				
			 }/* if */

			 if ( pCommandHandling->m_dtHandleInformation[i].Xfrms.ulFlags == TRANSFORM_VALID )
			 {				
				 float x, y, z;
				 x = pCommandHandling->m_dtHandleInformation[i].Xfrms.translation.x ;
				 y = pCommandHandling->m_dtHandleInformation[i].Xfrms.translation.y;
				 z = pCommandHandling->m_dtHandleInformation[i].Xfrms.translation.z;
				 sprintf( pszTemp, "%.2f", 
					 pCommandHandling->m_dtHandleInformation[i].Xfrms.translation.x );
				  ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 2, pszTemp);
				
				 sprintf( pszTemp, "%.2f", 
					 pCommandHandling->m_dtHandleInformation[i].Xfrms.translation.y );
				  ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 3, pszTemp);
			
				 sprintf( pszTemp, "%.2f", 
					 pCommandHandling->m_dtHandleInformation[i].Xfrms.translation.z );
				  ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 4, pszTemp);
				
				 sprintf( pszTemp, "%.4f", 
					 pCommandHandling->m_dtHandleInformation[i].Xfrms.rotation.q0 );
				  ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 5, pszTemp);
				 
				 sprintf( pszTemp, "%.4f", 
					 pCommandHandling->m_dtHandleInformation[i].Xfrms.rotation.qx );
				  ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 6, pszTemp);
				
				 sprintf( pszTemp, "%.4f", 
					 pCommandHandling->m_dtHandleInformation[i].Xfrms.rotation.qy );
				  ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 7, pszTemp);

				 sprintf( pszTemp, "%.4f", 
					 pCommandHandling->m_dtHandleInformation[i].Xfrms.rotation.qz );
				  ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 8, pszTemp);
	
				 sprintf( pszTemp, "%.4f", 
					 pCommandHandling->m_dtHandleInformation[i].Xfrms.fError );
				

				 if ( pCommandHandling->m_dtHandleInformation[i].HandleInfo.bPartiallyOutOfVolume )
					 ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 9, "POOV");
				 else if ( pCommandHandling->m_dtHandleInformation[i].HandleInfo.bOutOfVolume )
					  ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 9, "OOV");
				 else
					  ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 9, "OK");
				 RotationMatrixf m;
				 QuatRotation qr = pCommandHandling->m_dtHandleInformation[i].Xfrms.rotation;
				 CvtQuatToRotationMatrix(&qr, m);
				 vtkMRMLLinearTransformNode * transformNode = AuroraTransformNodeVec[i];
				 transformNode->GetMatrixTransformToParent()->SetElement(0, 3, x);
				 transformNode->GetMatrixTransformToParent()->SetElement(1, 3, y);
				 transformNode->GetMatrixTransformToParent()->SetElement(2, 3, z);

				 transformNode->GetMatrixTransformToParent()->SetElement(0, 0, m[0][0]);
				 transformNode->GetMatrixTransformToParent()->SetElement(0, 1, m[0][1]);
				 transformNode->GetMatrixTransformToParent()->SetElement(0, 2, m[0][2]);

				 transformNode->GetMatrixTransformToParent()->SetElement(1, 0, m[1][0]);
				 transformNode->GetMatrixTransformToParent()->SetElement(1, 1, m[1][1]);
				 transformNode->GetMatrixTransformToParent()->SetElement(1, 2, m[1][2]);

				 transformNode->GetMatrixTransformToParent()->SetElement(2, 0, m[2][0]);
				 transformNode->GetMatrixTransformToParent()->SetElement(2, 1, m[2][1]);
				 transformNode->GetMatrixTransformToParent()->SetElement(2, 2, m[2][2]);
				 if (IsCalibrating)
				 {
					 VersorType versor;
					 VectorType pos;
					 pos[0] = x;
					 pos[1] = y;
					 pos[2] = z;
					 versor.Set(qr.qx, qr.qy, qr.qz, qr.q0);
					 Pivot->RequestAddSample( versor, pos );
					 unsigned int num = Pivot->GetNumberOfSamples();
					 InvokeEvent(vtkCommand::ModifiedEvent, &num);
				 }
				/* if (i == 10)
					 transformNode->GetMatrixTransformToParent()->Invert();*/
				 transformNode->InvokeEvent(vtkMRMLLinearTransformNode::TransformModifiedEvent, pCommandHandling->m_dtHandleInformation[i].szPhysicalPort);
				
			 }/* if */
			 else 
			 {
				 if ( pCommandHandling->m_dtHandleInformation[i].Xfrms.ulFlags == TRANSFORM_MISSING )
					{
						ModuleGUI->GetReportMultiColumnList()->GetWidget()->InsertCellText(lineNum, 9, "Missing");
					}
				 else
				 {
					 //m_ctlTrackingList.SetItemText( nRow, 2, "DISABLED" );

				 } /* else */

			 }/* else */					
		 }/* if */
	 }/* for */
	 	 vtkKWTkUtilities::CreateTimerHandler(GetApplication(), 50, this, "Grasp");
 }

int EndoscopeLogic::GetCalibrationResult(double offset[4])
 {
	 Pivot->RequestCalculateCalibration();
	 if ( !Pivot->GetValidPivotCalibration())
	 {
		 GetApplication()->Script("messageBox -message \"Valid Calibration!\" ");
		 	Pivot->RequestReset();
		 return 0;
	 }
	 else
	 {
		 // Get the calibration transformation
		 VectorType translation = Pivot->GetCalibrationTransform().GetTranslation();
		 // Get the pivot focus position
		 PointType position = Pivot->GetPivotPosition();
		 // Get the calibration RMS error
		 ErrorType error = Pivot->GetRootMeanSquareError();
		 offset[0] = translation[0];
		 offset[1] = translation[1];
		 offset[2] = translation[2];
		 offset[3] = error;
		 // Dump the calibration class information
		/* std::cout << "PivotCalibration: " << std::endl;
		 std::cout << "NumberOfSamples: " << pivot->GetNumberOfSamples()
			 << std::endl;
		 std::cout << "Translation: " << translation << std::endl;
		 std::cout << "Pivot Position: " << position << std::endl;
		 std::cout << "Calibration RMS: " << error << std::endl;*/
		 Pivot->RequestReset();
	 }
	 return 1;
 }
